#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String  # 修改为接收字符串消息
import serial
import struct


class RobotHandINS(Node):
    def __init__(self):
        super().__init__('hand_control_node')  # ROS 2 使用 Node 类来初始化
        self.port_name = "/dev/ttyUSB0"  # 串口名称
        self.baud_rate = 115200
        self.ser = None
        self.open_serial_port()

        # 初始化指头位置
        self.finger_positions = [1900, 1900, 1900, 1900, 1900, 50]  # 假设每个指头的角度

        # 初始化 ROS 2 订阅器
        self.control_subscriber = self.create_subscription(
            String,
            '/hand_control_topic',
            self.control_hand_callback,
            10
        )

        # 定时器每隔1秒执行一次
        self.finger_index = 0  # 当前控制的指头索引
        self.is_reverse_order = False  # 控制顺序模式，False 为顺序，True 为逆序

        # 初始化定时器，默认周期为0.5秒
        self.timer_period = 0.5  # ROS 2 定时器周期使用秒数表示
        self.timer = self.create_timer(self.timer_period, self.control_finger_periodically)

    def __del__(self):
        if self.ser:
            self.close_serial_port()

    def open_serial_port(self):
        try:
            self.ser = serial.Serial(self.port_name, self.baud_rate, timeout=1)
            self.get_logger().info(f"Serial port {self.port_name} opened successfully.")
        except Exception as e:
            self.get_logger().error(f"Failed to open serial port {self.port_name}: {str(e)}")

    def close_serial_port(self):
        if self.ser:
            self.ser.close()
            self.get_logger().info(f"Serial port {self.port_name} closed.")

    def write_serial_port(self, data):
        try:
            self.ser.write(data)
            self.get_logger().info(f"Written data: {' '.join(f'{byte:02X}' for byte in data)}")
        except Exception as e:
            self.get_logger().error(f"Error writing to serial port: {str(e)}")
            return False
        return True

    def calculate_checksum(self, hex_array):
        checksum = sum(hex_array[2:]) & 0xFF
        return checksum

    def write_single_finger_position_cmd(self, ifinger_name, ifinger_position):
        output = [
            0x55, 0xAA, 0x04, ifinger_name, 0x03, 0x37,
            ifinger_position & 0xFF, (ifinger_position >> 8) & 0xFF
        ]
        checksum = self.calculate_checksum(output)
        output.append(checksum)

        self.get_logger().info(f"Generated frame: {' '.join(f'{byte:02X}' for byte in output)}")

        return self.write_serial_port(bytearray(output))

    def control_finger_periodically(self):
        if 0 <= self.finger_index < len(self.finger_positions):
            finger_position = self.finger_positions[self.finger_index]
            self.get_logger().info(f"Controlling finger {self.finger_index + 1} with position {finger_position}")
            self.write_single_finger_position_cmd(self.finger_index + 1, finger_position)

            # 根据顺序模式更新指头索引
            if self.is_reverse_order:
                self.finger_index -= 1  # 逆序模式，索引递减
            else:
                self.finger_index += 1  # 顺序模式，索引递增
        else:
            # 所有指头已控制完毕，重置指头索引
            if self.is_reverse_order:
                self.finger_index = len(self.finger_positions) - 1  # 逆序模式，重置为最后一个指头
            else:
                self.finger_index = 0  # 顺序模式，重置为第一个指头

    def control_hand_callback(self, msg):
        command = msg.data
        if command == "paper_grasp":
            self.get_logger().info("Executing paper_grasp...")
            self.finger_positions = [1050, 1150, 950, 950, 1900, 50]
            self.is_reverse_order = True  # 顺序模式
            self.finger_index = 0  # 重置为第一个指头
            self.timer_period = 0.3  # 抓取速度为0.5秒
            self.timer.cancel()  # 关闭当前定时器
            self.timer = self.create_timer(self.timer_period, self.control_finger_periodically)  # 重新启动定时器
        elif command == "release":
            self.get_logger().info("Executing release...")
            self.finger_positions = [1900, 1900, 1900, 1900, 950, 50]
            self.is_reverse_order = False  # 默认顺序模式
            self.finger_index = 0  # 重置为第一个指头
            self.timer_period = 0.1  # 释放速度为0.1秒
            self.timer.cancel()  # 关闭当前定时器
            self.timer = self.create_timer(self.timer_period, self.control_finger_periodically)  # 重新启动定时器
        elif command == "yogurt_grasp":
            self.get_logger().info("Executing yogurt_grasp...")
            self.finger_positions = [1200, 1200, 1200, 1200, 1900, 50]
            self.is_reverse_order = True  # 逆序模式
            self.finger_index = len(self.finger_positions) - 1  # 重置为最后一个指头
            self.timer_period = 0.2  # 抓取速度为0.5秒
            self.timer.cancel()  # 关闭当前定时器
            self.timer = self.create_timer(self.timer_period, self.control_finger_periodically)  # 重新启动定时器
        else:
            self.get_logger().warn(f"Unknown command: {command}")
            return


def main(args=None):
    rclpy.init(args=args)
    hand_control = RobotHandINS()
    rclpy.spin(hand_control)
    hand_control.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
